Introduction

This lab is similar the lab5, except change the linear distances to angular offsets. A lot of codes form lab 5 can be reused with a little modification.

Prelab

Start with lab5 code, replace distance-related variables with orientation variables:

float Setori;

float currealori;

float prerealori;

float curori;

float ori_t_gradient;

float preorierr; // previous orientation error

float curorierr;

 

after every function call to setVelocity(), I run

if (lognum < 1999)

                {

                    appendLog(curtime - startT, -1, -1, curori, -1, p, i, d, dt, preorierr);

                    lognum++;

                }

Where lognum is a int counter initialized as 0. This if condition, which is put inside the main loop, record the first 2000 logs, each of the log includes the running time since command was received, the current orientation, pid values, dt, and the previous orientation offset.

 After the car stop running, I used a loop similar to the lab5:

for (int logindex = 0; logindex < lognum; logindex++)

                {

                    sendLog(logindex);

                }

 

This sends all 2000 logs to python.

 

Lab

Sensor reading

digital motion processor (DMP) is used to turn the raw data from accelerometer, gyroscope, and compass to orientation readings. DMP is excellent for my purpose since “The DMP enables ultra-low power run-time and background calibration of the accelerometer, gyroscope, and compass” as ICM datasheet mentioned. With DMP used, I don’t need further background data calibration to prevent errors like the data shifting caused by gyroscope integration.

In this lab, I only need yaw, and the angle provided by DMP is in radian, so I removed the pitch and roll section and made following change to yaw:

double t3 = +2.0 * (q0 * q3 + q1 * q2);

                        double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);

                        currealori = atan2(t3, t4) * 180.0 / PI;

 

This takes t3, t4 provided by DMP and calculate the current real orientation(‘real’ is in relative to estimated orientation) in degree.

 

Videos of DMP readings and quaternion_visualization:

 

Change Set Orientation while running

My code is designed in the way such that the main actions are in the loop() with if conditions, and the commands sent from python simply changes these condition variables and then release the program to the rest part of loop() rather than trapping it in certain case within switch (cmd_type), thus the car can read commands while running.

 

case CHANGE_ORI:

        success = robot_cmd.get_next_value(Setori);

        if (!success)

            return;

        break;

 

video of Change Set Orientation while running:

 

plots of Change Set Orientation while running:

Low pass filter

As shown below, the raw d was very fuzzy, I implemented a low pass filter to reduce the high frequency noise:

 rawd = kd * ori_t_gradient;

                alpha_D = dt / (dt + 1000 / (2 * M_PI * fc));

                if (pred == -1.)

                {

                    d = rawd;

                    pred = d;

                }

                else

                {

                    // low pass filter

                    d = alpha_D * rawd + (1 - alpha_D) * pred;

                    pred = d;

                }

 

The alpha_D shows how much I trust the rawd; it’s calculated from fc using the same equations as the lab5: alpha= T/(T+R*C) and fc =1/(2pi*R*C); the 1000in the equation above is because the dt is in milisecond.   I continuously decrease critical frequency fc until I found fc= 10, which is the lowest fc that doesn’t cause too much delay—the formula above shows that the current d changes little from previous d if we have a too low alpha, which results in delay.

Fc=5:

Fc=10:

As the plots below shows: P is directly proportional to offset, so we can consider it as a reflection of offset; when P’s derivative changes from negative to positive, it means the offset also does the same, so D is expected to go across 0 at this point; in fc=10 plots, these 2 events happen almost in the same time, but in fc=5 plots, there’s a clearly observable delay between these 2 events. This confirms the fc value here cannot be further decreased.  

 

Final Result show

The kp, ki, and kd values I used are 1400|0|120. The heuristic doesn’t work well in lab5, so this time I start with setting p to a high value. The final value of kp is 1400, because various trials showed that this value let the car reach maximum speed (motor=255) at offset = 180; ki is again 0, since any non-zero values make the car run crazily, plus the car is always able to return to offset=0 without non-zero ki, which prove that non-zero ki is unnecessary; kd penalty high speed and help avoiding diverging, it is determined to be 120, with this value the car can run in the highest speed and still converging to offset=0.

The video of final show:

 

The images of final show:

As the video shows, the 3 highest sharp peaks in plots that reaches 180 corresponding to the 3 times I rotate the car over 180 degrees. And every time I spin the car, it’s able to reestablish at offset=0, as both the plots and video show. 

Range/Sampling time discussion

As mentioned previously, the first 2000 logs that’s composed of running time from starting running, the current orientation, pid values, dt, and the previous orientation offset are recorded and stored in the Artemis. 2000 logs, which provides 10~ 15 seconds logging, is enough for me to tunning the car. As the plots above shows, 2000 logs can shows 7 spins and reestablishing.

 

Something to ponder

I noticed that if the car starts running with a large offset, it will overshoot a lot and even spin for 360 degrees. To figure out the reason, I changed a lot of conditions to check whether this phenomenon changes, and I observed that this only happen with the non-zero initial orientation: actually, even if the car starts with a non-zero orientation and spin for 360 degrees, it still be able to settle at set orientation within 2 seconds and behave well for all later tests. This implies that the issue disappears soon after motor starts; I believe this is because the motor takes a few seconds to warm up. As the videos below shows, the car with initial offset=180 runs crazily, but with same conditions except hover for 2 seconds at the beginning, the car runs as expected.

Videos: